#include <boost/bind.hpp>
#include <boost/ref.hpp>
#include "ros/ros.h"
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>
#include <tf/tf.h>
#include "angle_utils.hh"

typedef const boost::function<void (const nav_msgs::Odometry::ConstPtr& msg)> InsCallback;


void msgCallback(const nav_msgs::Odometry::ConstPtr& msg, ros::Publisher &pub){
    tf::Quaternion q(
        msg->pose.pose.orientation.x,
        msg->pose.pose.orientation.y,
        msg->pose.pose.orientation.z,
        msg->pose.pose.orientation.w);

    geometry_msgs::Vector3 msg_rpy;
    tf::Matrix3x3 m(q);
    m.getRPY(msg_rpy.x, msg_rpy.y, msg_rpy.z);
    //msg_rpy.z = angles::normalize_angle(msg_rpy.z);
    pub.publish(msg_rpy);
}

int main(int argc, char *argv[])
{
    ros::init(argc,argv, "quat_to_rpy");
    ros::NodeHandle n;
    ros::Publisher pub = n.advertise<geometry_msgs::Vector3>("rpy", 100);
    InsCallback msg_handle= boost::bind(msgCallback, _1, boost::ref(pub));
    ros::Subscriber sub = n.subscribe("quat", 100, msg_handle);
    ros::spin();

    return 0;
}
